#include "sensor_type/gnss.h"

//静态成员变量必须在类外初始化
double GNSSData::origin_latitude = 0.0;
double GNSSData::origin_longitude = 0.0;
double GNSSData::origin_altitude = 0.0;
bool GNSSData::origin_position_inited = false;
GeographicLib::LocalCartesian GNSSData::geo_converter;


void GNSSData::InitOriginPosition()
{
    geo_converter.Reset(latitude, longitude, altitude); // 经纬度原点初始化

    origin_longitude = longitude;
    origin_latitude = latitude;
    origin_altitude = altitude;

    std::cout.precision(12);
    std::cout << "Gnss map origin: " <<  origin_longitude << " " 
                        << origin_latitude << " " << origin_altitude << std::endl;
    origin_position_inited = true;
}

void GNSSData::UpdateXYZ()
{
    if (!origin_position_inited)
    {
        std::cout<< "GeoConverter has not set origin position,now set it..." << std::endl;
    }
    geo_converter.Forward(latitude, longitude, altitude, local_E, local_N, local_U); //经纬度转ENU（米）
}

bool GNSSData::SyncData(std::deque<GNSSData> &UnsyncedData, std::deque<GNSSData> &SyncedData, double sync_time)
{
    // 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置
    // 即找到与同步时间相邻的左右两个数据
    // 需要注意的是，如果左右相邻数据有一个离同步时间差值比较大，则说明数据有丢失，时间离得太远不适合做差值
    while (UnsyncedData.size() >= 2)
    {
        if (UnsyncedData.front().time > sync_time)
            return false;
        if (UnsyncedData.at(1).time < sync_time)
        {
            UnsyncedData.pop_front();
            continue;
        }
        if (sync_time - UnsyncedData.front().time > 0.2)
        {
            UnsyncedData.pop_front();
            break;
        }
        if (UnsyncedData.at(1).time - sync_time > 0.2)
        {
            UnsyncedData.pop_front();
            break;
        }
        break;
    }
    if (UnsyncedData.size() < 2)
        return false;

    GNSSData front_data = UnsyncedData.at(0);
    GNSSData back_data = UnsyncedData.at(1);
    GNSSData synced_data;

    double front_scale = (back_data.time - sync_time) / (back_data.time - front_data.time);
    double back_scale = (sync_time - front_data.time) / (back_data.time - front_data.time);
    synced_data.time = sync_time;
    synced_data.status = back_data.status;
    synced_data.longitude = front_data.longitude * front_scale + back_data.longitude * back_scale;
    synced_data.latitude = front_data.latitude * front_scale + back_data.latitude * back_scale;
    synced_data.altitude = front_data.altitude * front_scale + back_data.altitude * back_scale;
    synced_data.local_E = front_data.local_E * front_scale + back_data.local_E * back_scale;
    synced_data.local_N = front_data.local_N * front_scale + back_data.local_N * back_scale;
    synced_data.local_U = front_data.local_U * front_scale + back_data.local_U * back_scale;

    SyncedData.push_back(synced_data);

    return true;
}
